% Florian Meyer, 22/04/15.

function [ controlOutput ] = infoSeekingController(anchorPos, agentPosterior, parameter)

%initialize parameters
[anchorNum, ~] = size(anchorPos);
particleNum = parameter.particlesNum;
[particleNumEstimation, ~, agentNum] = size(agentPosterior);
if(particleNum < particleNumEstimation)
    agentPosterior = agentPosterior(randperm(particleNumEstimation,particleNum),:,:);    
end
measurementVariance = parameter.measurementVariance;
measurementVariancePartner = measurementVariance/2;
observationMultiplier = parameter.observationMultiplier;

%initialize variouse vectors
observationValuesAnchors = zeros(particleNum,observationMultiplier,anchorNum,agentNum);
observationValuesAgents = zeros(particleNum,observationMultiplier,agentNum,agentNum);

%calculate controller output for each mobile agent
for agent = 1:agentNum
    %draw observation samples for future measurement corresponding to a partner anchor
    for anchor = 1:anchorNum
        tmp = [anchorPos(anchor,1)*ones(particleNum,1)-agentPosterior(:,1,agent) anchorPos(anchor,2)*ones(particleNum,1)-agentPosterior(:,2,agent)];
        tmpDist = sqrt(tmp(:,1).^2 + tmp(:,2).^2);
        measurementVarianceTmp = measurementVariance;
        if(parameter.pathloss)
            idx1 = tmpDist > parameter.distZero;
            idx2 = tmpDist <= parameter.distZero;
            measurementVarianceTmp = zeros(particleNum,observationMultiplier);
            measurementVarianceTmp(idx1,:) = repmat(((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance,1,observationMultiplier);
            measurementVarianceTmp(idx2,:) = measurementVariance;
        end
        observationValuesAnchors(:,:,anchor,agent) = repmat(tmpDist,1,observationMultiplier) + sqrt(measurementVarianceTmp).*randn(particleNum,observationMultiplier);
    end
    
    %draw observation samples for future measurement corresponding to a partner mobile agent
    for partnerAgent = 1:agentNum
        if (partnerAgent == agent)
            continue;
        end
        tmp = [agentPosterior(:,1,partnerAgent)-agentPosterior(:,1,agent) agentPosterior(:,2,partnerAgent)-agentPosterior(:,2,agent)];
        tmpDist = sqrt(tmp(:,1).^2 + tmp(:,2).^2);
        
        measurementVarianceTmp = measurementVariancePartner;
        if(parameter.pathloss)
            idx1 = tmpDist > parameter.distZero;
            idx2 = tmpDist <= parameter.distZero;
            measurementVarianceTmp = zeros(particleNum,observationMultiplier);
            measurementVarianceTmp(idx1,:) = repmat(((tmpDist(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariancePartner,1,observationMultiplier);
            measurementVarianceTmp(idx2,:) = measurementVariancePartner;
        end
        observationValuesAgents(:,:,partnerAgent,agent) = repmat(tmpDist,1,observationMultiplier) + sqrt(measurementVarianceTmp).*randn(particleNum,observationMultiplier);
    end
    
end
%calculate the mutual information gradient
controlOutput = calculateGradient(anchorPos, agentPosterior, observationValuesAnchors, observationValuesAgents, parameter);
end